# coding=utf-8
import cv2
import numpy as np
import time
import cv2
import numpy as np
import time
import threading
from util_3 import *
 
def nothing(x):
 
    pass

# cv2.namedWindow('image',cv2.WINDOW_AUTOSIZE)
cv2.namedWindow('image',cv2.WINDOW_FREERATIO)
cv2.resizeWindow('image', (640, 320))
# blue = np.uint8([[[255,0,0]]])
# hsv_blue = cv2.cvtColor(blue,cv2.COLOR_BGR2HSV)
# print(hsv_blue)


 
cv2.createTrackbar('Hue min','image',100,255,nothing)
cv2.createTrackbar('Hue max','image',124,255,nothing)
cv2.createTrackbar('sat min','image',43,255,nothing)
cv2.createTrackbar('sat max','image',255,255,nothing)
cv2.createTrackbar('val min','image',46,255,nothing)
cv2.createTrackbar('val max','image',255,255,nothing)
 
# 参数设置
W, H = 640, 480 # 图像宽高
# W, H = 1280, 720 # 图像宽高
FPS = 30 # 帧率
MAX_LENGTH = 1000 # 区分正面和侧面轮廓所使用的空间长度
RED = (0, 0, 255) # 红色
BLUE = (255, 0, 0) # 蓝色
GREEN = (0, 255, 0) # 绿色
# 设置hsv阈值
lower_blue = np.array([240, 33, 40])
# lower_blue = np.array([100, 43, 46])
upper_blue = np.array([250, 255, 255])
# upper_blue = np.array([124, 255, 255])
lower_red = np.array([0, 33, 40])
upper_red = np.array([10, 255, 255])
# 创建一个管道
pipeline = rs.pipeline() 
# 配置要流式传输的管道
config = rs.config()
config.enable_stream(rs.stream.color, W, H, rs.format.bgr8, 30) # 8位rgb图像
config.enable_stream(rs.stream.depth, W, H, rs.format.z16, 30) # 16位深度图像
# 左右双目
config.enable_stream(rs.stream.infrared, 1, W, H, rs.format.y8, 30) # 8位左红外图像
config.enable_stream(rs.stream.infrared, 2, W, H, rs.format.y8, 30) # 8位右红外图像
align = rs.align(rs.stream.color)
# 视野大，但有残缺
# align = rs.align(rs.stream.depth)
# 开始传输帧
profile = pipeline.start(config)
# 更改rgb相机的曝光值
color_sensor = profile.get_device().first_color_sensor()
depth_sensor = profile.get_device().first_depth_sensor()
# 关闭自动曝光
color_sensor.set_option(rs.option.enable_auto_exposure, 0)
depth_sensor.set_option(rs.option.enable_auto_exposure, 0)
# 设置曝光值
color_sensor.set_option(rs.option.exposure, 25)
depth_sensor.set_option(rs.option.exposure, 15000)
# 
depth_sensor = profile.get_device().first_depth_sensor()
# 
depth_scale = depth_sensor.get_depth_scale()



while(1):

    # 等待开启通道
    frames = pipeline.wait_for_frames()
    # 将深度图和RGB图对齐
    # frames = align.process(frames)
    # 获得RGB图像
    color_frame = frames.get_color_frame()
    # 未获得RGB图像
    if not color_frame:
        continue
    # 获得深度图像
    # depth_frame = frames.get_depth_frame()
    # 把图像像素转化为数组
    color_image = np.asanyarray(color_frame.get_data())
    # depth_image = np.asanyarray(depth_frame.get_data())
    # 深度图像转化为伪彩色图像(alpha数值越大对比度色差越大)
    # depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.21), cv2.COLORMAP_JET)
    # 转换到HSV (颜色空间转换) (色调H，饱和度S，亮度V)
    # hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)

    img = color_image
    hsv = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)
    h_min = cv2.getTrackbarPos('Hue min','image')
    h_max = cv2.getTrackbarPos('Hue max','image')
    s_min = cv2.getTrackbarPos('sat min','image')
    s_max = cv2.getTrackbarPos('sat max','image')
    v_min = cv2.getTrackbarPos('val min','image')
    v_max = cv2.getTrackbarPos('val max','image')
 
    lower = np.array([h_min,s_min,v_min])
    upper = np.array([h_max,s_max,v_max])
    mask = cv2.inRange(hsv,lower,upper)
    res = cv2.bitwise_and(img,img,mask=mask)
 
    # cv2.imshow('img',img)
    cv2.imshow('mask',mask)
    # cv2.imshow('res',res)
 
    k = cv2.waitKey(5)
 
    if k == 27:
 
        break
 
cv2.destroyAllWindows()